/*************************************************************************
*
*  sd84.c
*
*
*  Copyright 2008,2009 Steve Stancliff
*
*
*  Part of the TAOSF SSA system.
*
*
*  This module interfaces to the Devantech SD84 digital/analog/PWM i/o board
*
*
*  version 1.1.1, 4/24/09, SBS
*    - documentation
*    - cleanup
*
*  version 1.1, 1/9/09, SBS
*    - change from callback mode to polled
*
*  version 1.0, 1/8/09, SBS
*    - convert from standalone app to module
*    - LogPrint --> LogLevelPrint
*    - cleanup
*
*  version 0.1, 12/5/08, SBS
*    - first version based on compass.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/

/**
*  @file 
*  @ingroup plat_eng_hardware
*  @brief
*  This module interfaces to the Devantech SD84 
*    digital/analog/PWM i/o board
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <stdint.h>
#include "types.h"

#ifdef DEBUG
#include "logfile.h"
#endif

#include "cssl.h"

#include "sd84.h"

enum cmds { 
	SET_SERVO=1, 
	GET_SERVO, 
	SET_SERVOSPEED, 
	SET_MODE, 
	GET_MODE, 
	SET_ANALOG_COUNT, 
	GET_ANALOG_COUNT, 
	GET_DIGITAL_INPUT, 
	GET_ANALOG_INPUT, 
	GET_VERSION,
	PAD1, 
	PAD2, 
	PAD3, 
	PAD4, 
	PAD5, 
	PAD6, 
	PAD7, 
	PAD8, 
	PAD9, 
	PAD10, 
	GET_BULK, 
	TEST_MODE, 
	MAX_CMDS
};


enum errors {
	OK=0, 
	WARNING, 
	RANGE_ERROR, 
	MODE_ERROR 
};


enum modes { 
	MIN_MODES=20, 
	MODE_OUTPUT_LOW, 
	MODE_OUTPUT_HIGH, 
	MODE_INPUT_DIGITAL, 
	MODE_INPUT_ANALOG, 
	MODE_SERVO, 
	MAX_MODES 
};



static cssl_t *serial = NULL;

static int lastcommand;
static int lastargument;



/********************************************************
* Relationship between analog channels and port numbers
*
* 1 21
* 2 22
* 3 20
* 4 18
* 5 28
* 6 27
* 7 29
* 8 26
* 9 30
* 10 11
* 11 10
* 12 9
* 13 7
* 14 38
* 15 37
* 16 39
* 17 36
* 18 40
* 19 84
* 20 83
* 21 82
* 22 80
* 23 48
* 24 47
* 25 49
* 26 46
* 27 50
* 28 73
* 29 72
* 30 71
* 31 69
* 32 58
* 33 57
* 34 59
* 35 56
* 36 60
*
* i.e., if you set the number of analog channels = 5,
*   the analog ports will be (21,22,20,18,28)
*
*****************************************************/



/********************************************************
*
* sd84_parseresponse()
*/
/**
* @brief
*   Takes a string buffer and parses the response
*
*
********************************************************/
static void sd84_parseresponse(
			UBYTE *buf,
			int length)
{
	#ifdef DEBUG
	char logtemp[LOGSTRLEN];
	#endif
	

	switch(lastcommand){
		case GET_MODE:
		
			#ifdef DEBUG
			snprintf(logtemp,sizeof(logtemp)
						,"sd84_parseresponse(): pin #%d  ",lastargument);
			logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
			#endif
			
			switch(buf[0]){
				case	MODE_OUTPUT_LOW:
					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_VERBOSE
							,"sd84_parseresponse(): mode = output low");
					#endif
					break;
				case MODE_OUTPUT_HIGH:
					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_VERBOSE
							,"sd84_parseresponse(): mode = output high");
					#endif
					break;
				case MODE_INPUT_DIGITAL:
					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_VERBOSE
							,"sd84_parseresponse(): mode = input digital");
					#endif
					break;
				case MODE_INPUT_ANALOG:
					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_VERBOSE
							,"sd84_parseresponse(): mode = input analog");
					#endif
					break;
				case MODE_SERVO:
					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_VERBOSE
							,"sd84_parseresponse(): mode = servo");
					#endif
					break;
			}
			break;
			
		case GET_DIGITAL_INPUT:
			if(length==1){
				#ifdef DEBUG
				snprintf(logtemp,sizeof(logtemp)
							,"sd84_parseresponse(): pin #%d  state = %d\n",lastargument,buf[0]);
				logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
				#endif
			}
			break;
			
		case GET_VERSION:
			if(length==2){
				#ifdef DEBUG
				snprintf(logtemp,sizeof(logtemp)
							,"sd84_parseresponse(): board #%d  revision #%d\n",buf[0],buf[1]);
				logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
				#endif
			}
			break;

		case SET_SERVO:	
			if(length==1){
				#ifdef DEBUG
				snprintf(logtemp,sizeof(logtemp)
							,"sd84_parseresponse(): set servo #%d  rval = %d\n",lastargument,buf[0]);
				logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
				#endif
			}else{
				#ifdef DEBUG
				snprintf(logtemp,sizeof(logtemp)
							,"sd84_parseresponse(): 'set servo' wrong response length\n");
				logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
				#endif
			}
			break;

		case GET_SERVO:	
			if(length==2){
				UWORD i;
				UBYTE *j;

				j=(UBYTE *)&i;

				*j = buf[0];
				*(j+1) = buf[1];
				
				#ifdef DEBUG
				snprintf(logtemp,sizeof(logtemp)
							,"sd84_parseresponse(): get servo #%d  value = %d\n",lastargument,i);
				logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
				#endif
			}else{
				#ifdef DEBUG
				snprintf(logtemp,sizeof(logtemp)
							,"sd84_parseresponse(): 'get servo' wrong response length\n");
				logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
				#endif
			}
			break;

		case SET_MODE:	
			if(length==1){
				#ifdef DEBUG
				snprintf(logtemp,sizeof(logtemp)
							,"sd84_parseresponse(): set mode #%d  rval = %d\n",lastargument,buf[0]);
				logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
				#endif
			}
			break;

	}
	
	return;
}


/********************************************************
*
* sd84_set_servo()
*/
/**
* @brief
*   Output a servo position command to the sd84
*
*
* Parameters:
*   - channel  - sd84 servo channel
*   - position - -100 = full left ; +100 = full right
*
* Returns:
*   - none
*
********************************************************/
void sd84_set_servo(
		int channel
		,int position)
{
		uint8_t buf[8];
		uint8_t *j;
		uint16_t value;

//	printf("percent = %d\n",percent);

//	value = 1000 + 10*percent;	
		value = 1500 + -5*position;	
//	printf("value = %d\n",value);
	
		j = (uint8_t *)&value;
	
//	printf("MSB:LSB = %d:%d\n"
//			,*j
//			,*(j+1));

//	printf("MSB:LSB = %x:%x\n"
//			,*j
//			,*(j+1));


		buf[0] = 0xaa;		
		buf[1] = 0xa0;
		buf[2] = 0x55;
		buf[3] = SET_SERVO;
		buf[4] = (uint8_t)channel;	
		buf[5] = 0x02;			
		buf[6] = *j;				// low byte
		buf[7] = *(j+1);			// high byte
	
		cssl_putdata(serial,buf,8);

		lastcommand = SET_SERVO;
		lastargument = channel;


	{
	int length = -1;
	UBYTE buf[512];
//	register int i;

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

/*		for(i=0;i<length;i++){
			printf("%d",buf[i]);
			if(i+1<length){
				printf(":");
			}
		}
		printf("\n");
*/		
		sd84_parseresponse(buf,length);
	}
	}
	
	return;
}


/********************************************************
*
* sd84_get_servo()
*/
/**
* @brief
*   Request a servo position from the sd84
*
*
* Parameters:
*   - channel  - sd84 servo channel
*
* Returns:
*   - none
*
********************************************************/
void sd84_get_servo(
		int channel)
{
	{
	UBYTE buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_SERVO;
	buf[4] = (uint8_t)channel;			
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_SERVO;
	lastargument = channel;
	}

	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));


	if(length>0){

		sd84_parseresponse(buf,length);

		}
	}

	return;
}


/********************************************************
*
* sd84_set_servo_speed()
*/
/**
* @brief
*   Set servo speed
*
*
* Parameters:
*   - channel  - sd84 servo channel
*   - speed    - speed for servo
*
* Returns:
*   - none
*
********************************************************/
void sd84_set_servo_speed(
		int channel
		,int speed)
{
	{
	UBYTE buf[7];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_SERVOSPEED;
	buf[4] = (uint8_t)channel;			
	buf[5] = 1;			
	buf[6] = (uint8_t)speed;			

	cssl_putdata(serial,buf,7);

	lastcommand = SET_SERVOSPEED;
	lastargument = channel;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	
	return;
}


/********************************************************
*
* sd84_set_mode()
*/
/**
* @brief
*   Set sd84 channel mode
*
*
* Parameters:
*   - channel - sd84 channel
*   - mode    - mode to set
*
* Returns:
*   - none
*
********************************************************/
void sd84_set_mode(
		int channel
		,int mode)
{
	{
	UBYTE buf[7];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_MODE;
	buf[4] = (uint8_t)channel;
	buf[5] = 1;			
	buf[6] = (uint8_t)mode;			

	cssl_putdata(serial,buf,7);

	lastcommand = SET_MODE;
	lastargument = channel;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_set_mode_servo()
*/
/**
* @brief
*   Sets a sd84 channel as a servo output
*
*
* Parameters:
*   - channel - sd84 channel
*
* Returns:
*   - none
*
********************************************************/
void sd84_set_mode_servo(
		int channel)
{
	{
	UBYTE buf[7];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_MODE;
	buf[4] = (uint8_t)channel;
	buf[5] = 1;			
	buf[6] = (uint8_t)MODE_SERVO;			

	cssl_putdata(serial,buf,7);

	lastcommand = SET_MODE;
	lastargument = channel;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_get_mode()
*/
/**
* @brief
*   Request the mode of an sd84 channel
*
*
* Parameters:
*   - channel - sd84 channel
*
* Returns:
*   - none
*
********************************************************/
void sd84_get_mode(
		int channel)
{
	{
	UBYTE buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_MODE;
	buf[4] = (uint8_t)channel;	
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_MODE;
	lastargument = channel;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_set_analog_channels()
*/
/**
* @brief
*   Set sd84 channels to analog out
*
*
* Parameters:
*   - count - number of analog channels
*
* Returns:
*   - none
*
********************************************************/
void sd84_set_analog_channels(
		int count)
{
	{
	UBYTE buf[7];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_ANALOG_COUNT;
	buf[4] = 0;	
	buf[5] = 1;			
	buf[6] = (uint8_t)count;	
	
	cssl_putdata(serial,buf,7);

	lastcommand = SET_ANALOG_COUNT;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_get_analog_channels()
*/
/**
* @brief
*   Request number of analog channels 
*
*
* Parameters:
*   - none
*
* Returns:
*   - none
*
********************************************************/
void sd84_get_analog_channels(void)
{
	{
	UBYTE buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_ANALOG_COUNT;
	buf[4] = 0;	
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_ANALOG_COUNT;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_get_input_digital()
*/
/**
* @brief
*   Request current value from a digital channel
*
*
* Parameters:
*   - channel - sd84 channel
*
* Returns:
*   - none
*
********************************************************/
void sd84_get_input_digital(
		int channel)
{
	{
	UBYTE buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_DIGITAL_INPUT;
	buf[4] = (uint8_t)channel;
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_DIGITAL_INPUT;
	lastargument = channel;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_get_input_analog()
*/
/**
* @brief
*   Request current value from an analog channel
*
*
* Parameters:
*   - channel - sd84 channel
*
* Returns:
*   - none
*
********************************************************/
void sd84_get_input_analog(
		int channel)
{
	{
	UBYTE buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_ANALOG_INPUT;
	buf[4] = (uint8_t)channel;
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_ANALOG_INPUT;
	lastargument = channel;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_get_version()
*/
/**
* @brief
*   Request board version from sd84
*
*
* Parameters:
*   - none
*
* Returns:
*   - none
*
********************************************************/
void sd84_get_version(void)
{
	{
	UBYTE buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_VERSION;
	buf[4] = 2;			// CPU number
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_VERSION;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);

	}
	}
	return;
}


/********************************************************
*
* sd84_get_bulk()
*/
/**
* @brief
*   Request all input from a single sd84 CPU
*
*
* Parameters:
*   - cpu - number of cpu to query
*
* Returns:
*   - none
*
********************************************************/
void sd84_get_bulk(
		int cpu)
{
	{
	UBYTE buf[6];

	if(cpu<1 || cpu>4){
		#ifdef DEBUG
		logfile_LogLevelPrint(LOGLEVEL_INFO,"get_bulk() - invalid CPU number");
		#endif
		printf("get_bulk() - invalid CPU number\n");
		return;
	}
	
	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_BULK;
	buf[4] = (uint8_t)cpu;			
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_BULK;
	lastargument = cpu;
	}
	{
	int length = -1;
	UBYTE buf[512];

	length = cssl_getdata(serial,buf,sizeof(buf));
	if(length>0){

		sd84_parseresponse(buf,length);
	
	}
	}
	return;
}


/********************************************************
*
* sd84Init()
*/
/**
* @brief
*   Initialize the cssl library 
*
*
*
********************************************************/
static int sd84Init(
			char *port
			,int baud
			,int bits
			,int stopbits)
{
	cssl_start();
    
	serial = cssl_open(
				port
				,NULL
				,0
				,baud
				,bits
				,0
				,1);

				
	
	if (!serial) {
		printf("sd84Init(): %s\n",cssl_geterrormsg());
		return(-1);
	}

	cssl_settimeout(serial, 1);

	return(0);
}


/********************************************************
*
* sd84_Start()
*/
/**
* @brief
*   Called by external code to initialize this module
*
*
*
********************************************************/
int sd84_Start(
	char *port)
{
	int rval;

	int baud = 115200;
	int bits = 8;
	int stopbits = 2;
	
	printf("sd84_Start()\n");

	#ifdef DEBUG
	logfile_LogLevelPrint(LOGLEVEL_INFO," ");
	logfile_LogLevelPrint(LOGLEVEL_INFO,"*******************************************");		
	logfile_LogLevelPrint(LOGLEVEL_INFO,"SD84 STARTUP");
	logfile_LogLevelPrint(LOGLEVEL_INFO,"*******************************************");		
	logfile_LogLevelPrint(LOGLEVEL_INFO," ");
	#endif

	rval = sd84Init(port,baud,bits,stopbits);

	printf("sd84_Start(): done\n");
	return(rval);
}
